#include "pcl/io/pcd_io.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"


struct EIGEN_ALIGN16 PointOut
{
	PCL_ADD_POINT4D;
	float intensity;
	float time;
	uint16_t ring;
	PCL_MAKE_ALIGNED_OPERATOR_NEW
};



POINT_CLOUD_REGISTER_POINT_STRUCT(PointOut,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (float, time, time)
    (uint16_t, ring, ring)
)

struct EIGEN_ALIGN16 PointIn
{
	PCL_ADD_POINT4D;
	PCL_ADD_INTENSITY_8U
	uint16_t ring;
	double timestamp;
	PCL_MAKE_ALIGNED_OPERATOR_NEW
};

POINT_CLOUD_REGISTER_POINT_STRUCT(PointIn,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (uint8_t, intensity, intensity)
	(uint16_t, ring, ring)
	(double, timestamp, timestamp)
)



class Convert : public rclcpp::Node
{
  public:
	std::string _input_topic = "rslidar_points";
	std::string _output_topic = "velodyne_points";
	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr input;
	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr output;
	void input_handler(sensor_msgs::msg::PointCloud2 msg)
	{
		pcl::PointCloud<PointIn> pcl_in;
		pcl::PointCloud<PointOut> pcl_out;
		PointOut pout;
		pcl::fromROSMsg(msg, pcl_in);
		double start = pcl_in.front().timestamp;
		for (auto pin : pcl_in)
		{	
			pout.x = pin.x;
			pout.y = pin.y;
			pout.z = pin.z;
			pout.intensity = (float)pin.intensity / UINT8_MAX;
			pout.ring = pin.ring;
			pout.time = pin.timestamp - start;
			pcl_out.push_back(pout);
		}

		sensor_msgs::msg::PointCloud2 out;
		pcl::toROSMsg(pcl_out, out);
		out.header.frame_id = msg.header.frame_id;
		out.header.stamp = msg.header.stamp;
		output->publish(out);
		RCLCPP_INFO(this->get_logger(), "last point time %lf %lf", pcl_in.back().timestamp, pcl_out.back().time);
	}

	Convert(std::string name) : Node(name)
	{
		this->declare_parameter("input_topic", _input_topic);
		this->declare_parameter("output_topic", _output_topic);
		this->get_parameter("input_topic", _input_topic);
		this->get_parameter("output_topic", _output_topic);

		input = this->create_subscription<sensor_msgs::msg::PointCloud2>(
			_input_topic, 1, std::bind(&Convert::input_handler, this, std::placeholders::_1));
		output = this->create_publisher<sensor_msgs::msg::PointCloud2>(_output_topic, 1);
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<Convert>("pcl_converter");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
